package motor;

import lejos.nxt.*;

public class carrito implements libcart{
	
	NXTRegulatedMotor m1;//MOTOR_IZQ
	NXTRegulatedMotor m2;//MOTOR_DER
	private boolean isAgent;
	
	
	//Asumimos que las ruedas van a estar en los puertos A y C de los motores
	public carrito(){
		m1 = new NXTRegulatedMotor(MotorPort.A);
		m2 = new NXTRegulatedMotor(MotorPort.C);
		
		m1.setSpeed(250);
		m2.setSpeed(250);
		
		setAgent(false);
		 
	}

	//Devuelvo -1 si la velocidad de los motores es distinta
	
	public int GetPresentSpeed() {
		// TODO Auto-generated method stub		
		if (m1.getSpeed()!=m2.getSpeed()){
			return -1;			
		}else{
			return m1.getSpeed();
		}
		
	}

	
	public void SetMovingSpeed(int speed) {
		// TODO Auto-generated method stub
		m1.setSpeed(speed);
		m2.setSpeed(speed+15);
		
	}


	public void Forward() {
		// TODO Auto-generated method stub
		m1.forward();
		m2.forward();
		
	}

	
	public void Backward() {
		// TODO Auto-generated method stub
		m1.backward();
		m2.backward();
		
	}

	public void resetTacho(){
		m1.resetTachoCount();
		m2.resetTachoCount();
	}
	
	public int getTacho(){
		return (m1.getTachoCount() + m2.getTachoCount())/2;
	}
	
	public boolean Moving() {
		// TODO Auto-generated method stub
		return m1.isMoving() && m2.isMoving();
	}
	
	
	public void TurnLeft(int degree) {
		
		// TODO Auto-generated method stub
		m1.resetTachoCount();
		m2.resetTachoCount();
		m1.rotate(degree,true);
		m2.rotate(-degree,false);
	}	
	
	
	public void TurnRight(int degree) {
		// TODO Auto-generated method stub
		m1.resetTachoCount();
		m2.resetTachoCount();
		m1.rotate(-degree,true);
		m2.rotate(degree,false);
		
	}

	
	public void Spin() {
		// TODO Auto-generated method stub
		m1.backward();
		m2.forward();
		}

	public void Stop() {
		// TODO Auto-generated method stub
		int vel = m1.getSpeed();
		m1.setSpeed(0);
		m2.setSpeed(0);
		m2.stop();
		m1.stop();
		this.SetMovingSpeed(vel);	
	}

	
	public int GetPosition(int m) {
		// TODO Auto-generated method stub
		if (m == MOTOR_IZQ){
			return m1.getPosition();
		}else{
			return m2.getPosition();
		}
		
	}

	public void SpinLeft() {
		// TODO Auto-generated method stub
		m1.backward();
		m2.forward();
		
	}

	public void SpinRight() {
		// TODO Auto-generated method stub
		m1.forward();
		m2.backward();
	}

	public void setAgent(boolean isAgent) {
		this.isAgent = isAgent;
	}

	public boolean isAgent() {
		return isAgent;
	}

	


}
